#include "MainSceneWidget.h"
#include <qgl.h>
#include <gl/glu.h>
#include "ReconstructProcess.h"
#include <glm/glm.hpp>
#include <glm/gtx/transform.hpp>
#include <QMouseEvent>
#include "SensorDataProcessedInput.h"
bool drawModelColor = false;
bool modelUpdated = true;
bool drawOverlapCloud = true;
bool drawModel = false;

MainSceneWidget::MainSceneWidget (int argc, char *argv[], QWidget *parent) :
		QGLWidget (QGLFormat (
		QGL::DoubleBuffer | QGL::DepthBuffer | QGL::Rgba), parent),
		g_camera (), _cameraInterface (g_camera), g_isPause (true),
		g_isTex (true)
{
	for (int i = 0; i < 9; ++i)
		_isModelShow[i] = true;
	_parseCommandLineArgs (argc, argv);
}

MainSceneWidget::~MainSceneWidget ()
{

}

void MainSceneWidget::initializeGL ()
{
	glEnable (GL_DEPTH_TEST);

	g_camera.setDistance (20);
	g_camera.setViewpoint (glm::vec3 (0, 0, 0));
	g_camera.setCameraAngle (45, -45);

}

void MainSceneWidget::paintGL ()
{
	_processSensorData ();

	// Clear the OpenGL buffers
	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glPushAttrib (GL_ENABLE_BIT);

	// Setup the OpenGL viewpoint
	glViewport (0, 0, width (), height ());
	glMatrixMode (GL_PROJECTION);
	glPushMatrix ();
	glLoadIdentity ();
	gluPerspective (45.0, (double)width () / height (), 1.0, 5000);

	glMatrixMode (GL_MODELVIEW);
	glPushMatrix ();
	g_camera.setSight ();

 	_drawViewports ();
	_drawCoord ();
	_setLight ();
// 	_drawModelObjects ();
	_drawPointClouds ();

	glPopMatrix ();
	glMatrixMode (GL_PROJECTION);
	glPopMatrix ();

	glPopAttrib ();

	update ();
}

void MainSceneWidget::_drawViewports ()
{
	for (size_t i = 0; i < g_reconstructProcess.getSensorsCount (); ++i) {
		const SensorDataInput *sensor = g_reconstructProcess.getSensor (i);
// 		double frameVFOV = sensor->getSensorParameter ().frameVFOV;
// 		double frameHFOV = sensor->getSensorParameter ().frameHFOV;
// 		unsigned short frameDepth = sensor->getSensorParameter ().frameDepth;
//
// 		glm::mat4x4 perspectiveMat = glm::perspective< float > (
// 				glm::degrees (frameVFOV), frameHFOV / frameVFOV,
// 				1.0, frameDepth - 1);
// 		glm::mat4x4 inversePerspectiveMat = glm::inverse (perspectiveMat);
// 		glm::mat4x4 mat = sensor->getToWorldMat () * inversePerspectiveMat;
		glm::vec3 t1 = sensor->unprojectToWorld (glm::vec3 (0.0f, 0.0f, 10.0f));
		glm::vec3 t2 = sensor->unprojectToWorld (glm::vec3 (640.0, 0.0f, 10.0f));
		glm::vec3 t3 = sensor->unprojectToWorld (glm::vec3 (640.0, 480.0f, 10.0f));
		glm::vec3 t4 = sensor->unprojectToWorld (glm::vec3 (0.0f, 480.0f, 10.0f));
		glm::vec3 t5 = sensor->unprojectToWorld (glm::vec3 (0.0f, 0.0f, 10000.0f));
		glm::vec3 t6 = sensor->unprojectToWorld (glm::vec3 (640.0f, 0.0f, 10000.0f));
		glm::vec3 t7 = sensor->unprojectToWorld (glm::vec3 (640.0f, 480.0f, 10000.0f));
		glm::vec3 t8 = sensor->unprojectToWorld (glm::vec3 (0.0f, 480.0f, 10000.0f));

		if (_isModelShow[i])
			glColor3f (1.0f, 1.0f, 1.0f);
		else
			glColor3f (0.2f, 0.2f, 0.2f);
		glBegin (GL_LINES);
		glVertex3fv (&t1[0]); glVertex3fv (&t2[0]);
		glVertex3fv (&t2[0]); glVertex3fv (&t3[0]);
		glVertex3fv (&t3[0]); glVertex3fv (&t4[0]);
		glVertex3fv (&t4[0]); glVertex3fv (&t1[0]);

		glVertex3fv (&t5[0]); glVertex3fv (&t6[0]);
		glVertex3fv (&t6[0]); glVertex3fv (&t7[0]);
		glVertex3fv (&t7[0]); glVertex3fv (&t8[0]);
		glVertex3fv (&t8[0]); glVertex3fv (&t5[0]);

		glVertex3fv (&t1[0]); glVertex3fv (&t5[0]);
		glVertex3fv (&t2[0]); glVertex3fv (&t6[0]);
		glVertex3fv (&t3[0]); glVertex3fv (&t7[0]);
		glVertex3fv (&t4[0]); glVertex3fv (&t8[0]);
		glEnd ();
	}
}

void MainSceneWidget::_processSensorData ()
{
	if (!g_isPause) {
		if (!g_reconstructProcess.grabNewFrameAndProcess ())
			close ();
		modelUpdated = false;
	}
	if (!modelUpdated) {
// 		g_reconstructProcess.generateModels ();
		modelUpdated = true;
	}
}

void MainSceneWidget::_setLight ()
{
	glEnable (GL_NORMALIZE);
	glEnable (GL_LIGHTING);
	glEnable (GL_LIGHT0);
 	float lp[] = { 1.0f, 1.0f, 1.0f, 0.0f };
	float lc[] = { 1.0f, 1.0f, 1.0f, 1.0f };
 	glLightfv (GL_LIGHT0, GL_POSITION, lp);
	glLightfv (GL_LIGHT0, GL_AMBIENT, lc);
	glLightfv (GL_LIGHT0, GL_DIFFUSE, lc);
	glLightModeli (GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE);
}

void MainSceneWidget::mousePressEvent (QMouseEvent * e)
{
	if (e->button () & Qt::LeftButton)
		_cameraInterface.mouseDown (CameraInterface::B_LEFT,
				e->pos ().x (), e->pos ().y ());
	if (e->button () & Qt::MidButton)
		_cameraInterface.mouseDown (CameraInterface::B_MIDDLE,
				e->pos ().x (), e->pos ().y ());
	if (e->button () & Qt::RightButton)
		_cameraInterface.mouseDown (CameraInterface::B_RIGHT,
				e->pos ().x (), e->pos ().y ());
}

void MainSceneWidget::mouseMoveEvent (QMouseEvent * e)
{
	_cameraInterface.mouseDrag (e->pos ().x (), e->pos ().y ());
}

void MainSceneWidget::mouseReleaseEvent (QMouseEvent * e)
{
	if (e->button () & Qt::LeftButton)
		_cameraInterface.mouseUp (CameraInterface::B_LEFT,
				e->pos ().x (), e->pos ().y ());
	if (e->button () & Qt::MidButton)
		_cameraInterface.mouseUp (CameraInterface::B_MIDDLE,
				e->pos ().x (), e->pos ().y ());
	if (e->button () & Qt::RightButton)
		_cameraInterface.mouseUp (CameraInterface::B_RIGHT,
				e->pos ().x (), e->pos ().y ());
}

void MainSceneWidget::wheelEvent (QWheelEvent * e)
{
	_cameraInterface.mouseWheel (e->delta () / 120);
}

void MainSceneWidget::keyPressEvent (QKeyEvent * e)
{
	_cameraInterface.keyDown (e->key () - 'A' + 'a');
	switch (e->key ()) {
	case Qt::Key_Escape:
		close ();
	case Qt::Key_P:
		g_isPause = !g_isPause; break;
	case Qt::Key_T:
		g_isTex = !g_isTex; break;
	case Qt::Key_1:
		_isModelShow[0] = !_isModelShow[0]; break;
	case Qt::Key_2:
		_isModelShow[1] = !_isModelShow[1]; break;
	case Qt::Key_3:
		_isModelShow[2] = !_isModelShow[2]; break;
	case Qt::Key_4:
		_isModelShow[3] = !_isModelShow[3]; break;
	case Qt::Key_L:
		drawModelColor = !drawModelColor; break;
	case Qt::Key_O:
		drawOverlapCloud = !drawOverlapCloud; break;
	}
}

void MainSceneWidget::keyReleaseEvent (QKeyEvent * e)
{

}

void MainSceneWidget::_drawCoord ()
{
	glBegin (GL_LINES);
	glColor3f (1, 0, 0); glVertex3f (0, 0, 0); glVertex3f (8, 0, 0);
	glColor3f (0, 1, 0); glVertex3f (0, 0, 0); glVertex3f (0, 6, 0);
	glColor3f (0, 0, 1); glVertex3f (0, 0, 0); glVertex3f (0, 0, 10);
	glEnd ();

	glColor3f (1, 1, 1);
	glPushMatrix ();
	glScalef (15.0, 15.0, 15.0);

// 	glBegin (GL_LINES);
// 	glVertex3f (-1, -1, -1); glVertex3f (1, -1, -1);
// 	glVertex3f (1, -1, -1); glVertex3f (1, 1, -1);
// 	glVertex3f (1, 1, -1); glVertex3f (-1, 1, -1);
// 	glVertex3f (-1, 1, -1); glVertex3f (-1, -1, -1);
// 	glVertex3f (-1, -1, 1); glVertex3f (1, -1, 1);
// 	glVertex3f (1, -1, 1); glVertex3f (1, 1, 1);
// 	glVertex3f (1, 1, 1); glVertex3f (-1, 1, 1);
// 	glVertex3f (-1, 1, 1); glVertex3f (-1, -1, 1);
// 	glVertex3f (-1, -1, -1); glVertex3f (-1, -1, 1);
// 	glVertex3f (1, -1, -1); glVertex3f (1, -1, 1);
// 	glVertex3f (1, 1, -1); glVertex3f (1, 1, 1);
// 	glVertex3f (-1, 1, -1); glVertex3f (-1, 1, 1);
// 	glEnd ();
	
	glPopMatrix ();
}

void MainSceneWidget::_drawModelObjects ()
{
// 	glEnable (GL_COLOR_MATERIAL);
// 	glEnable (GL_BLEND);
// 	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
// 	if (g_isTex)
// 		glEnable (GL_TEXTURE_2D);
// 	else
// 		glDisable (GL_TEXTURE_2D);
// 	float colors[2][3] = {{0.5, 0.5, 0.5}, {0.5, 0.0, 0.0}};
// 	std::vector< ModelObject * > modelObjs = g_reconstructProcess.getModels ();
// 	for (size_t i = 0; i < modelObjs.size (); ++i) {
// 		if (!_isModelShow[i])
// 			continue;
// 		Model *model = modelObjs[i]->getModel ();
// 		if (model->getFaceCount () == 0)
// 			continue;
// 		GLuint texId = bindTexture (*g_reconstructProcess.getTextures ()[i]);
// 
// 		if (drawModelColor)
// 			glColor3fv (colors[i]);
// 		else
// 			glColor3f (0.5, 0.5, 0.5);
// 		//	glDisable (GL_LIGHTING);
// 		glEnableClientState (GL_VERTEX_ARRAY);
// 		glEnableClientState (GL_NORMAL_ARRAY);
// 		glEnableClientState (GL_TEXTURE_COORD_ARRAY);
// 		// 	glEnableClientState (GL_COLOR_ARRAY);
// 		glVertexPointer (3, GL_FLOAT, 0, &model->getVertices ()[0][0]);
// 		glNormalPointer (GL_FLOAT, 0, &model->getNormals ()[0][0]);
// 		glTexCoordPointer (2, GL_FLOAT, 0, &model->getTexCoords ()[0][0]);
// 		//	glColorPointer (3, GL_FLOAT, 0, &normals[0][0]);
// 		glDrawElements (GL_TRIANGLES, model->getFaceCount () * 3,
// 				GL_UNSIGNED_INT, &model->getFaces ()[0][0]);
// 		//	glDisable (GL_LIGHTING);
// 		// 	glColor3f (1.0, 0.0, 0.0);
// 		// 	glDrawArrays (GL_POINTS, 0, vertexs.size ());
// 		glDisableClientState (GL_VERTEX_ARRAY);
// 		glDisableClientState (GL_NORMAL_ARRAY);
// 		glDisableClientState (GL_TEXTURE_COORD_ARRAY);
// 		// 	glDisableClientState (GL_COLOR_ARRAY);
// 		deleteTexture (texId);
// 	}
// 	glBindTexture (GL_TEXTURE_2D, 0);
// 	glDisable (GL_TEXTURE_2D);
// 	glDisable (GL_BLEND);
// 	glDisable (GL_COLOR_MATERIAL);
}

void MainSceneWidget::_drawPointClouds ()
{
	glPushAttrib (GL_ENABLE_BIT);
	glPushAttrib (GL_CURRENT_BIT);
	glDisable (GL_LIGHTING);
	glDisable (GL_TEXTURE_2D);
// 	glEnable (GL_COLOR_MATERIAL);

	float colors[2][3] = {{0.5, 0.5, 0.5}, {0.5, 0.0, 0.0}};
	std::vector< std::vector< Vertex > > &verticesClouds =
			g_reconstructProcess.getVerticesClouds ();
	for (size_t i = 0; i < verticesClouds.size (); ++i) {
		if (!_isModelShow[i])
			continue;
		if (verticesClouds[i].size () == 0)
			continue;

		if (drawModelColor)
			glColor3fv (colors[i]);
		else
			glColor3f (0.5, 0.5, 0.5);
		glEnableClientState (GL_VERTEX_ARRAY);
		//	glEnableClientState (GL_NORMAL_ARRAY);
		glEnableClientState (GL_COLOR_ARRAY);
		glVertexPointer (3, GL_FLOAT, sizeof (Vertex), &verticesClouds[i][0].point[0]);
		//	glNormalPointer (GL_FLOAT, sizeof (Vertex), &model->getNormals ()[0][0]);
		glColorPointer (3, GL_FLOAT, sizeof (Vertex), &verticesClouds[i][0].color[0]);
		glDrawArrays (GL_POINTS, 0, verticesClouds[i].size ());
		glDisableClientState (GL_VERTEX_ARRAY);
		//	glDisableClientState (GL_NORMAL_ARRAY);
		glDisableClientState (GL_COLOR_ARRAY);
	}

	if (drawOverlapCloud) {
		for (size_t i = 0; i < g_reconstructProcess.getOverlapClouds ().size (); ++i) {
			if (i == 0)
				glColor3f (0, 1, 0);
			else
				glColor3f (0, 0, 1);
			glEnableClientState (GL_VERTEX_ARRAY);
			//	glEnableClientState (GL_NORMAL_ARRAY);
// 			glEnableClientState (GL_COLOR_ARRAY);
			glVertexPointer (3, GL_FLOAT, sizeof (Vertex), &g_reconstructProcess.getOverlapClouds ()[i][0].point[0]);
			//	glNormalPointer (GL_FLOAT, sizeof (Vertex), &model->getNormals ()[0][0]);
// 			glColorPointer (3, GL_FLOAT, sizeof (Vertex), &g_reconstructProcess.getOverlapClouds ()[i][0].color[0]);
			glDrawArrays (GL_POINTS, 0, g_reconstructProcess.getOverlapClouds ()[i].size ());
			glDisableClientState (GL_VERTEX_ARRAY);
			//	glDisableClientState (GL_NORMAL_ARRAY);
// 			glDisableClientState (GL_COLOR_ARRAY);
		}
	}

	glPopAttrib ();
	glPopAttrib ();
}

void MainSceneWidget::_parseCommandLineArgs (int argc, char *argv[])
{
	// exe [num] (-f [datafile] [intrinsicfile] | -k [instrinsicfile]){num}
	if (argc < 2)
		throw std::exception ("MainSceneWidget::_parseCommandLineArgs(): "
		"Command line argument error.");

	int sensorCount = atoi (argv[1]);
	if (sensorCount <= 0)
		throw std::exception ("MainSceneWidget::_parseCommandLineArgs(): "
		"Sensor Count must be greater than 0.");

	for (int i = 0, argId = 2; i < sensorCount; ++i) {
		SensorDataInput *pSensorInput;
		bool isKinect;
		if (strcmp (argv[argId], "-k") == 0 && argc - argId >= 2)
			isKinect = true;
		else if (strcmp (argv[argId], "-f") == 0 && argc - argId >= 3)
			isKinect = false;
		else
			throw std::exception ("MainSceneWidget::_parseCommandLineArgs(): "
			"Command line argument error.\n");
		++argId;

		char strIndex[2]; itoa (i, strIndex, 10);
		if (isKinect)
			pSensorInput = new SensorDataKinectInput (
			std::string ("Sensor ") + strIndex);
		else
			pSensorInput = new SensorDataFileInput (
			std::string ("Sensor ") + strIndex, argv[argId++]);
		pSensorInput->setIntrinsicFromFile (argv[argId++]);
		g_reconstructProcess.addSensor (
				/*new SensorDataProcessedInput (*/pSensorInput/*)*/);
	}
}
